/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package luft27.comm;

import java.io.IOException;
import java.nio.BufferUnderflowException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;
import luft27.propertytree.PropertyNode;
import luft27.propertytree.PropertyTree;
import luft27.propertytree.PropertyValue;
import luft27.util.RunnableLoop;

/**
 *
 * @author amalikov
 */
public class FCSLink extends RunnableLoop {
	
	public FCSLink(SerialStream s) {
		this.proto = new Protocol(s);
		needInfo = true;
		powerModuleIndex = 0;
		cycleCnt = 0;
	}
	
	@Override
	public void init() {
		PropertyTree.INSTANCE.set("/linkstatus/goodcnt", new PropertyValue(0));
		PropertyTree.INSTANCE.set("/linkstatus/errcnt", new PropertyValue(0));
		v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		v.set("/route/current/needupdate", new PropertyValue(true));
	}

	@Override
	public void cycle() {
		try {
			limitPacketRate();
			
			sendCommand(v.getNode("/commands/auto"), CMD_AUTO);
			sendCommand(v.getNode("/commands/manual"), CMD_MANUAL);
			sendCommand(v.getNode("/commands/futaba"), CMD_FUTABA);
			sendCommand(v.getNode("/commands/abort"), CMD_ABORT);
			sendCommand(v.getNode("/commands/emergency"), CMD_EMERGENCY);
			
			if (needInfo)
				updateInfo(v);
			
			updateRoute(v);
				
			if (v.get("/state").asInt() == STATE_MANUAL_DATALINK) {
				updateManualCtrl(v);
					
				if (cycleCnt == 2) {
					updateTelemetry(v);
					updateTelemetry2(v);
				}
					
			} else {
				updateTelemetry(v);
				updateTelemetry2(v);
				updatePowerModules(v);
				updateActiveLeg(v);
			}

			if (cycleCnt == 4)
				updatePowerModules(v);

			if (cycleCnt == 6)
				updateActiveLeg(v);
				
			if (++cycleCnt > 10)
				cycleCnt = 0;
		} catch (IOException ex) {
			Logger.getLogger(FCSLink.class.getName()).log(Level.SEVERE, null, ex);
		}
	}
	
	private void limitPacketRate() {
		final long minCycleTime = 20;
		long delta = minCycleTime - (System.nanoTime() - lastCycleTime) / 1000000;
		
		if (delta > 0 ) {
			try {
				Thread.sleep(delta);
			} catch (InterruptedException ex) {
			}
		}
		
		lastCycleTime = System.nanoTime();
	}
	
	private void updateInfo(final PropertyNode v) throws IOException {
		request(new Packet((byte)0, (byte)ID_INFO, new byte[0]), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				v.set("/name", new PropertyValue(new String(data, 1, data.length - 1)));
				needInfo = false;
				return true;
			}
		});
	}
	
	private void updateTelemetry(final PropertyNode v) throws IOException {
		request(new Packet((byte)0, (byte)ID_TELEMETRY, new byte[0]), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				v.set("/state", new PropertyValue((short)b.get() & 0xFF));
				v.set("/orientation/roll", new PropertyValue(b.getShort() * 0.01));
				v.set("/orientation/pitch", new PropertyValue(b.getShort() * 0.01));
				v.set("/orientation/heading", new PropertyValue(((int)b.getShort() & 0xFFFF) * 0.01));
				v.set("/airdata/ias", new PropertyValue(b.getShort() * 0.01));
				v.set("/airdata/altitude", new PropertyValue(b.getShort()));
				v.set("/airdata/vs", new PropertyValue(b.getShort() * 0.01));
				v.set("/directalt/altitude", new PropertyValue(b.getShort() * 0.01));
				v.set("/position/course", new PropertyValue(((int)b.getShort() & 0xFFFF) * 0.01));
				v.set("/position/gs", new PropertyValue(((int)b.getShort() & 0xFFFF) * 0.01));
				v.set("/position/latitude", new PropertyValue(b.getFloat()));
				v.set("/position/longitude", new PropertyValue(b.getFloat()));
				v.set("/position/altitude", new PropertyValue(b.getShort()));
				v.set("/engine/rpm", new PropertyValue((int)b.getShort() & 0xFFFF));
				v.set("/engine/t1", new PropertyValue(b.getShort()));
				v.set("/engine/t2", new PropertyValue(b.getShort()));
				v.set("/controls/ailerons", new PropertyValue(b.get()));
				v.set("/controls/elevator", new PropertyValue(b.get()));
				v.set("/controls/rudder", new PropertyValue(b.get()));
				v.set("/controls/throttle", new PropertyValue(b.get()));
				v.set("/controls/flaps", new PropertyValue(b.get()));
				v.set("/track/waypointindex", new PropertyValue((short)b.get() & 0xFF));
				v.set("/track/z", new PropertyValue(b.getShort()));
				v.set("/track/s", new PropertyValue(b.getShort() * 10));
				v.set("/updatetrigger", new PropertyValue(1));
				return true;
			}
		});
	}

	private void updateTelemetry2(final PropertyNode v) throws IOException {
		request(new Packet((byte)0, (byte)ID_TELEMETRY2, new byte[0]), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				v.set("/failures", new PropertyValue((long)b.getInt() & 0xFFFFFFFF));
				v.set("/electric/servo/uavg", new PropertyValue(b.getShort() * 0.01));
				v.set("/electric/servo/itotal", new PropertyValue(b.getShort() * 0.01));
				v.set("/engine/fuel", new PropertyValue((short)b.get() & 0xFF));
				v.set("/updatetrigger", new PropertyValue(1));
				return true;
			}
		});
	}

	private void updatePowerModules(final PropertyNode v) throws IOException {
		request(new Packet((byte)0, (byte)ID_POWERMODULES, new byte[]{(byte)powerModuleIndex}), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				PropertyNode m = v.getNode("/electric/modules/module[" + powerModuleIndex + "]");
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				m.set("/status", new PropertyValue((short)b.get() & 0xFF));
				m.set("/failures", new PropertyValue((short)b.get() & 0xFF));
				m.set("/uin", new PropertyValue(b.getShort() * 0.1));
				m.set("/uout", new PropertyValue(b.getShort() * 0.1));
				m.set("/ibat", new PropertyValue(b.getShort() * 0.01));
				m.set("/iout", new PropertyValue(b.getShort() * 0.01));
				m.set("/tmod", new PropertyValue(b.getShort()));
				m.set("/tbat", new PropertyValue(b.getShort()));
				
				if (++powerModuleIndex >= NUM_OF_POWER_MODULES)
					powerModuleIndex = 0;
				
				return true;
			}
		});		
	}

	private void updateActiveLeg(final PropertyNode v) throws IOException {
		request(new Packet((byte)0, (byte)ID_ACTIVELEG, new byte[0]), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				v.set("/track/leg/from/latitude", new PropertyValue(b.getFloat()));
				v.set("/track/leg/from/longitude", new PropertyValue(b.getFloat()));
				v.set("/track/leg/from/altitude", new PropertyValue(b.getShort()));
				v.set("/track/leg/to/latitude", new PropertyValue(b.getFloat()));
				v.set("/track/leg/to/longitude", new PropertyValue(b.getFloat()));
				v.set("/track/leg/to/altitude", new PropertyValue(b.getShort()));
				v.set("/updatetrigger", new PropertyValue(1));
				return true;
			}
		});
	}

	private void updateManualCtrl(final PropertyNode v) throws IOException {
		PropertyNode m = v.getNode("/manualcontrols");
		byte[] manual = new byte[] {
			(byte)(m.get("/ailerons").asDouble() * 127),
			(byte)(m.get("/elevator").asDouble() * 127),
			(byte)(m.get("/rudder").asDouble() * 127),
			(byte)(m.get("/throttle").asDouble() * 127),
			(byte)m.get("/flaps").asInt()
		};
		
		request(new Packet((byte)0, (byte)ID_MANUALCTRL, manual), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				v.set("/orientation/roll", new PropertyValue(b.getShort() * 0.01));
				v.set("/orientation/pitch", new PropertyValue(b.getShort() * 0.01));
				v.set("/orientation/heading", new PropertyValue(((int)b.getShort() & 0xFFFF) * 0.01));
				v.set("/airdata/ias", new PropertyValue(b.getShort() * 0.01));
				v.set("/airdata/altitude", new PropertyValue(b.getShort()));
				v.set("/airdata/vs", new PropertyValue(b.getShort() * 0.01));
				v.set("/directalt/altitude", new PropertyValue(b.getShort() * 0.01));
				v.set("/updatetrigger", new PropertyValue(1));
				return true;
			}
		});
	}

	private void sendCommand(PropertyNode node, int cmd) throws IOException {
		if (!node.get().asBool())
			return;

		node.set(new PropertyValue(false));
		
		request(new Packet((byte)0, (byte)ID_COMMAND, new byte[]{(byte)cmd}), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				return true;
			}
		});
	}
	
	private void updateRoute(PropertyNode v) throws IOException {
		PropertyNode node = v.getNode("/route/current");
		if (!node.get("/needupdate").asBool())
			return;
		
		final List<Wpt> wpts = new ArrayList<>();
		
		int i = 0;
		boolean eof = false;
		while (!eof) {
			Wpt wpt = readWaypoint(i++);
			wpts.add(wpt);
			eof = wpt.alt == -32768;	// end marker
		}
			
		i = 0;
		for (Wpt wpt : wpts) {
			final PropertyNode w = node.getNode("/wpt[" + i++ + "]");
			w.set("/latitude", new PropertyValue(wpt.lat));
			w.set("/longitude", new PropertyValue(wpt.lon));
			w.set("/altitude", new PropertyValue(wpt.alt));
			w.set("/present", new PropertyValue(wpt.alt != -32768));
		}
		
		node.set("/needupdate", new PropertyValue(false));
		node.set(new PropertyValue(true));	// update trigger
	}
	
	private Wpt readWaypoint(final int i) throws IOException {
		int t = 0;
		final Wpt wpt = new Wpt();
		while (!request(new Packet((byte)0, (byte)ID_WPTGET, new byte[]{(byte)i}), new Handler(){
				@Override
				public boolean handle(byte[] data) {
					ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);

					if ((byte)i != b.get())
						return false;

					wpt.lat = b.getFloat();
					wpt.lon = b.getFloat();
					wpt.alt = b.getShort();
					return true;
				}
			})) {
			if (++t >= 10)
				throw new IOException();
		}
		
		return wpt;
	}

	private void updateOisCtrl(final PropertyNode v) throws IOException {
		byte[] ctrl = new byte[]{
			(byte)(v.get("/ois/controls/azimuth").asDouble() * 127),
			(byte)(v.get("/ois/controls/elevation").asDouble() * 127),
			(byte)(v.get("/ois/controls/zoom").asDouble() * 127),
			(byte)(v.get("/ois/controls/flags").asInt()),
		};
		
		request(new Packet((byte)0, (byte)ID_OISCTRL, ctrl), new Handler(){
			@Override
			public boolean handle(byte[] data) {
				ByteBuffer b = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
				v.set("/ois/state/azimuth", new PropertyValue(b.getShort() * 0.01));
				v.set("/ois/state/elevation", new PropertyValue(b.getShort() * 0.01));
				v.set("/ois/state/fov", new PropertyValue(b.getShort() * 0.01));
				v.set("/updatetrigger", new PropertyValue(1));
				return true;
			}
		});
	}

	private interface Handler {
		boolean handle(byte[] data);
	}	
	
	private boolean request(Packet req, Handler handler) throws IOException {
		PropertyNode goodCnt = PropertyTree.INSTANCE.getNode("/linkstatus/goodcnt");
		PropertyNode errCnt = PropertyTree.INSTANCE.getNode("/linkstatus/errcnt");
		
		proto.send(req);
		
		for (int i = 0; i < 10; ++i) {	// trying to recover from getting an "old" answer for the request
			Packet ans = proto.receive();
			
			if (ans == null)
				break;

			if ((ans.id == req.id + 1) && (ans.addr == req.addr || req.addr == 0)) {
				goodCnt.set(new PropertyValue(goodCnt.get().asInt() + 1));
				try {
					return handler.handle(ans.data);
				} catch (BufferUnderflowException ex) {
					Logger.getLogger(FCSLink.class.getName()).log(Level.WARNING, null, ex);
					return false;
				}
			}
		}
		
		errCnt.set(new PropertyValue(errCnt.get().asInt() + 1));
		return false;
	}

	private class Wpt {
		public float lat;
		public float lon;
		public short alt;
	}
	
	private static final int ID_INFO = 0x00;
	private static final int ID_TELEMETRY = 0x10;
	private static final int ID_TELEMETRY2 = 0x12;
	private static final int ID_POWERMODULES = 0x14;
	private static final int ID_ACTIVELEG = 0x16;
	private static final int ID_MANUALCTRL = 0x20;
	private static final int ID_COMMAND = 0x22;
	private static final int ID_PARAMGET = 0x30;
	private static final int ID_PARAMSET = 0x32;
	private static final int ID_WPTGET = 0x40;
	private static final int ID_WPTSET = 0x42;
	private static final int ID_WPTGOTO = 0x48;
	private static final int ID_WPTOPER = 0x4A;
	private static final int ID_ADSSET = 0x50;
	private static final int ID_ADSRESET = 0x52;
	private static final int ID_OISCTRL = 0x80;
	
	private static final int STATE_MANUAL_DATALINK = 0xC1;
	
	private static final int CMD_AUTO = 0x00;
	private static final int CMD_ENGINESTART = 0x01;
	private static final int CMD_TAKEOFF = 0x02;
	private static final int CMD_ABORT = 0x0F;
	private static final int CMD_FUTABA = 0x55;
	private static final int CMD_EMERGENCY = 0xAA;
	private static final int CMD_MANUAL = 0xFF;
	
	private PropertyNode v;
	private final Protocol proto;
	private boolean needInfo;
	private int powerModuleIndex;
	private static final int NUM_OF_POWER_MODULES = 7;
	private int cycleCnt;
	
	private long lastCycleTime = 0;
}
